clc;
clear;
close all;
r0 = 0.55 * 16 * 2 * pi;
k = 0.55;
L_haed = 3.41;
L_body = 2.2;
v_head = 1;
t_total = 300;
dt = 1;
x_positions = zeros(223,t_total+1);
y_positions = zeros(223,t_total+1);
velocities = zeros(223,t_total+1);
for t = 0:t_total
    theta_head = v_head * t / r0;
    r_head = r0 + k * theta_head;
    x_head = r_head * cos(theta_head);
    y_head = r_head * sin(theta_head);
    x_positions(1,t+1) = x_head;
    y_positions(1,t+1) = y_head;
    for i = 2:223
        L = (i-1) * L_body + L_haed;
        theta_i = theta_head - (L-L_haed) / r0;
        r_i = r_head;
        x_i = r_i * cos(theta_i);
        y_i = r_i * sin(theta_i);
        x_positions(i,t+1) = x_i;
        y_positions(i,t+1) = y_i;
        v_i = k * v_head / r_i;
        velocities(i,t+1) = v_i * 100;
    end
end
disp('位置和速度计算完成')